#include "Com_PID.h"

//
void ResetPID(PidObject ** pidObjects, uint8_t len)
{
    for (uint8_t i = 0; i < len; i++)
    {
        pidObjects[i] -> prevError = 0;
        pidObjects[i] -> integ = 0;
        pidObjects[i] -> out = 0;
    }
}

void PID_Update(PidObject *pid, float dt)
{
    float temp_err;
    float dri;
    //计算偏差
    temp_err = pid->desired - pid->measured;
    //计算积分
    pid->integ += temp_err * dt;
    //计算微分
    dri = (temp_err- pid->prevError) / dt;
    //带入公式计算输出
    pid->out = pid->kp * temp_err + pid->ki * pid->integ + pid->kd * dri; 
    //本次偏差值作为上一次偏差值
    pid->prevError = temp_err;
}

//串级pid，外环角度的输出作为内环角速度的期望值（输入）
void CasecadePID(PidObject *pidAngle, PidObject *pidRate, float dt)
{
    /* 1、角度外环进行PID处理 */
    PID_Update(pidAngle,dt);
    /* 2、外环的输出，赋值给内环的期望值 */
    pidRate -> desired = pidAngle ->out;
    /* 3、对内环进行PID计算 */
    PID_Update(pidRate,dt);
}
